
#include "motor_task.h"



void Motor_task(void const * argument)
{
	uint8_t flag_f=0;
    head_form_init();
    /*
    
	*/
    pwm_init();
    hx_data_1.motor_pos_set=950;//940
    hx_data_1.motor_ang_set=160;
    hx_data_2.motor_pos_set=220;//220
    hx_data_2.motor_ang_set=30;
    hx_data_1.motor_pos_set=angle_chage_1(hx_data_1.motor_ang_set);
    hx_data_2.motor_pos_set=angle_chage_2(hx_data_2.motor_ang_set);
    while(1)
    {
		/**/
			if(rc_ctrl.rc.s[1]==2 || rc_ctrl.rc.s[1]==3){
				if(flag_f==0){
					motor_stop();
				}
				
			}
			else if(rc_ctrl.rc.s[1]==1){
        hx_data_1.motor_pos_set=angle_chage_1(hx_data_1.motor_ang_set);
        hx_data_2.motor_pos_set=angle_chage_2(hx_data_2.motor_ang_set);
//				if(flag_f==0){
					motor_run(hx_data_1.motor_pos_set,hx_data_2.motor_pos_set,40);
//				}
			}
//			Duoj_Con(dj_ang_set_0,0);
//			Duoj_Con(dj_ang_set_1,1);
//			Duoj_Con(dj_ang_set_2,2);
//			Duoj_Con(dj_ang_set_3,3);
        /*
		if(data_get_from_hx_flag){
            send_flag=1;
            send_id=1;
            for(j=0;j<59;j++){
                send_Dats[j]=0x00;
            }
            send_Dats[0]=data_get_from_hx[6];
            send_Dats[1]=data_get_from_hx[7];
            send_Dats[2]=data_get_from_hx[9];
            send_Dats[3]=data_get_from_hx[10];
            data_get_from_hx_flag=0;
        }
        */
    osDelay(1);
//			if(flag_f==1){
//			motor_read();
//			}
//			flag_f++;
//			if(flag_f==2){
//				flag_f=0;
//			}
    if(data_get_from_hx_flag==1){
        hx_data_1.motor_pos_read=(data_get_from_hx[6] | (data_get_from_hx[7]<<8));
        hx_data_2.motor_pos_read=(data_get_from_hx[9] | (data_get_from_hx[10]<<8));
        data_get_from_hx_flag=0;
    }
		
    }
    
}


